//
// Created by 孟令涛 on 24-7-15.
//
#include "CarControlTask.h"
#include "cmsis_os2.h"
#include "CarMovementTask.h"
#include "GlobalVars.h"
#include "utils.h"
//#include "cmsis_compiler.h"
#define ZhiZou 200
#define ZhuanWan 300
#define XuanZhuan 300

int a = 0;

bool ReadSensor(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin) {
    a = !HAL_GPIO_ReadPin(GPIOx, GPIO_Pin);
    return !HAL_GPIO_ReadPin(GPIOx, GPIO_Pin); // 低电平表示检测到黑线
}
// CarControlTask.c





int last_state = 0;
int find_state = 0;
bool left1, left2, right1, right2;
uint16_t left_speed = 200;
uint16_t right_speed = 200;

void myCode() {
    int lfCode = carControlLeft1 * 1000 + carControlLeft2 * 100 + carControlRight1 * 10 + carControlRight2;

    switch (lfCode) {
        // 十字路口
        case 1111:
            carControl_left_speed = 175;
            carControl_right_speed = 175;
            movement_state = 1;
            break;
            // 前进
        case 0110:
            carControl_left_speed = 175;
            carControl_right_speed = 175;
            movement_state = 1;
            break;
            // 左转
        case 0100:
            carControl_left_speed = 0;
            carControl_right_speed = 150;
            movement_state = 3;
            break;
            // 右转
        case 0010:
            carControl_left_speed = 150;
            carControl_right_speed = 0;
            movement_state = 4;
            break;
            // 左直角
        case 1110:
            carControl_left_speed = 300;
            carControl_right_speed = 300;
            movement_state = 5;
            break;
            // 右直角
        case 0111:
            carControl_left_speed = 300;
            carControl_right_speed = 300;
            movement_state = 6;
            break;
        default:
            carControl_left_speed = 175;
            carControl_right_speed = 175;
            movement_state = 1;
            break;
    }
}

void HuCode() {
    // 十字路口
    if (left1 &&/* left2 && right1 && */right2) {
        carControl_left_speed = ZhiZou;
        carControl_right_speed = ZhiZou;
        control_movement_state = 1;
    }



        //锐角左转
        /*else if (left1 && left2 && right1) {
                carControl_left_speed = 300;
                carControl_right_speed = 300;
                return 5;
        }
        //锐角右转
        else if (left2 && right1 && right2) {
                carControl_left_speed = 300;
                carControl_right_speed = 300;
                return 6;
        } */
        //钝角左转
    else if (left1) {
        carControl_left_speed = XuanZhuan;
        carControl_right_speed = XuanZhuan;
        control_movement_state = 5;
    }
        //钝角右转
    else if (right2) {
        carControl_left_speed = XuanZhuan;
        carControl_right_speed = XuanZhuan;
        control_movement_state = 6;
    }
        //左微调
    else if (left2 && !right1) {
        carControl_left_speed = 0;
        carControl_right_speed = ZhuanWan;
        control_movement_state = 3;
    }
        //右微调
    else if (!left2 && right1) {
        carControl_left_speed = ZhuanWan;
        carControl_right_speed = 0;
        control_movement_state = 4;
    }
        //直走
    else if (left2 && right1) {
        carControl_left_speed = ZhiZou;
        carControl_right_speed = ZhiZou;
        control_movement_state = 1;
    }
        //直走
    else {
        carControl_left_speed = ZhiZou;
        carControl_right_speed = ZhiZou;
        control_movement_state = 1;
    }
}


void CarControlTask(void *argument) {
    for (;;) {
        if (currentMode == MODE_LINE_FOLLOWING) {
            // 读取传感器状态
            left1 = ReadSensor(GPIOC, GPIO_PIN_14);
            left2 = ReadSensor(GPIOC, GPIO_PIN_13);
            right1 = ReadSensor(GPIOC, GPIO_PIN_15);
            right2 = ReadSensor(GPIOB, GPIO_PIN_12);

            EnterCriticalSection();
            carControlLeft1 = left1;
            carControlLeft2 = left2;
            carControlRight1 = right1;
            carControlRight2 = right2;
            ExitCriticalSection();
            find_state = 0;
//        myCode();
            HuCode();
        }
        osDelay(5);
    }

}


